﻿using System;
using System.Linq;
using Microsoft.Xna.Framework;

namespace SpaceGame.Gameplay.AI.Behaviors
   
{
    /// <summary>
    /// Class is responsible for implementing steering behaviours.
    /// </summary>
    /// <remarks>Based on ideas from Craig Reynolds' http://www.red3d.com/cwr/steer/</remarks>
    class SteeringBehavior : Behavior
    {
        #region Consts
        /// <summary>
        /// Set entity max speed to use when clamping.
        /// </summary>
        private const float max_speed = 10f;

        /// <summary>
        /// the distance at which slowing begins.
        /// </summary>
        private const float slowing_distance = 150f;

        /// <summary>
        /// Set radar sensitivity
        /// </summary>
        private const float minDistanceToCollision = (10 * max_speed) * (10 * max_speed);

        /// <summary>
        /// Max force when avoiding obstacles.
        /// </summary>
        private const float max_force =  2f;

        /// <summary>
        /// Factor that indicates heading of awareness for obstacles.
        /// </summary>
        private const float awareness_factor = 40f;
        #endregion

        public SteeringBehavior(IEntity ship)
            : base(ship)
        {
        }

        /// <summary>
        /// Clamp velocity to smooth arrival at target.
        /// </summary>
        /// <param name="target"></param>
        /// <returns></returns>
        public Vector2 Arrival(Vector2 target, Vector2 forward)
        {
            var target_offset = target - Entity.Position;
            var distance = target_offset.Length();

            if(distance < slowing_distance)
            {
                return -forward + forward * ((distance + Entity.Radius) / slowing_distance);
            }
            
            return Vector2.Zero;

			#region cooments2
			/*
            var ramped_speed = max_speed*(distance/slowing_distance);
            var clipped_speed = Math.Min(ramped_speed, max_speed);
            var desired_velocity = (clipped_speed / distance) * forward;
            var steering = desired_velocity - forward;
            return steering;
             */
			#endregion
        }

        public Vector2 AvoidObstacles(Vector2 newDirection)
        {
            var forward = newDirection; 
            forward.Normalize();

            //todo: refactor
           
            var head_radar = forward * awareness_factor;
            head_radar.X += Entity.Position.X;
            head_radar.Y += Entity.Position.Y;

			#region comments
			/*
            var degree = MathHelper.PiOver4/2;
            var head_radar1 = forward * awareness_factor;
            head_radar1.X = (float) (Math.Cos(degree)*head_radar1.X - head_radar1.Y*Math.Sin(degree));
            head_radar1.Y = (float) (Math.Sin(degree)*head_radar1.X + head_radar1.Y *Math.Sin(degree));
            head_radar1.X += Entity.Position.X;
            head_radar1.Y += Entity.Position.Y;

            var head_radar2 = forward * awareness_factor;
            head_radar2.X = (float)(Math.Cos(degree) * head_radar2.X + head_radar2.Y * Math.Sin(degree));
            head_radar2.Y = (float)(-Math.Sin(degree) * head_radar2.X + head_radar2.Y * Math.Sin(degree));
            head_radar2.X += Entity.Position.X;
            head_radar2.Y += Entity.Position.Y;
             */
			#endregion

			Vector2 avoidance = Vector2.Zero;
            Vector2 positionObstacle = Vector2.Zero;
            float? nearestDistanceSquared = null;
            
           // test all obstacles for intersection with my forward axis,
            // select the one whose point of intersection is nearest
            foreach (Entity obstacle in Entity.World.Entities)
            {
                if (!ConsiderEntityAnObstacle(obstacle)) continue;

                var possibleCollision = Collision.CircleLineCollide(obstacle.Position, obstacle.Radius, Entity.Position, head_radar);
                possibleCollision |= Collision.CircleCircleIntersect(obstacle.Position, obstacle.Radius, Entity.Position, Entity.Radius*2f);
                
                if (!possibleCollision) continue;
                
                var distanceSquared = Vector2.DistanceSquared(obstacle.Position, Entity.Position);
                if(!nearestDistanceSquared.HasValue || distanceSquared < nearestDistanceSquared.Value)
                {
                    nearestDistanceSquared = distanceSquared;
                    positionObstacle = obstacle.Position;
                }
			}

			#region awoid walls comments
			//TODO: also avoid walls
            //avoid head on collisions with walls
			/*
			for (int wall = 0; wall < Entity.World.Walls.Length / 2; wall++)
			{
				Vector2 collisionPoint;
				var possibleCollision = Collision.LineLineIntersect(Entity.World.Walls[wall * 2], Entity.World.Walls[wall * 2 + 1], Entity.Position, head_radar, out collisionPoint);
				if (!possibleCollision) continue;

				var distanceSquared = Vector2.DistanceSquared(collisionPoint, Entity.Position);
				if (!nearestDistanceSquared.HasValue || distanceSquared < nearestDistanceSquared.Value)
				{
					nearestDistanceSquared = distanceSquared;
					positionObstacle = collisionPoint;
				}
			}
			 */
			#endregion

			// when a nearest intersection was found
            if (nearestDistanceSquared.HasValue && nearestDistanceSquared.Value < minDistanceToCollision)
            {
                // compute avoidance steering force: take offset from obstacle to me,
                // take the component of that which is lateral (perpendicular to my
                // forward direction), set length to maxForce, add a bit of forward
                // component (in capture the flag, we never want to slow down)
                var offset = Entity.Position - positionObstacle;
                
                //perpendicular component
                avoidance = offset - (Vector2.Dot(offset, forward) * forward);
                avoidance.Normalize();
                avoidance *= max_force;

                //avoidance += forward;
            }

            return avoidance;
        }


        /// <summary>
        /// Policy to determine which entities are harmful and should avoid collision.
        /// </summary>
        /// <param name="entity"></param>
        /// <returns></returns>
        public bool ConsiderEntityAnObstacle(IEntity entity)
        {
            return !(Entity == entity
                     || entity is PowerUp
                     || (entity is Projectile && ((Projectile)entity).Owner == Entity));
        }
    }
}
